/*
 * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
 *
 * See the NOTICE file(s) distributed with this work for additional
 * information regarding copyright ownership.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contact: mosaic@fokus.fraunhofer.de
 */
package org.eclipse.mosaic.fed.carla.clojure.facades;

import libpython_clj2.java_api;
import org.eclipse.mosaic.fed.carla.clojure.ClojureCarlaConnection;
import org.eclipse.mosaic.fed.carla.clojure.commands.*;
import org.eclipse.mosaic.fed.carla.clojure.complex.ClojureSimulationStepResult;
import org.eclipse.mosaic.interactions.application.CoSimulationStep;
import org.eclipse.mosaic.interactions.traffic.VehicleUpdates;
import org.eclipse.mosaic.interactions.vehicle.CarlaVehicleRegistration;
import org.eclipse.mosaic.lib.geo.MutableCartesianPoint;
import org.eclipse.mosaic.lib.objects.UnitData;
import org.eclipse.mosaic.lib.objects.vehicle.CarlaVehicleData;
import org.eclipse.mosaic.lib.objects.vehicle.VehicleData;
import org.eclipse.mosaic.lib.util.NameGenerator;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.stream.Collectors;

public class ClojureSimulationFacade {

    private final Logger log = LoggerFactory.getLogger(this.getClass());

    private final ClojureCarlaConnection carlaConnection;

    private final SimulationSimulateStep simulateStep;

    private final VehicleGet vehicleGet;

    private final VehicleAdd vehicleAdd;

    private final VehicleRemove vehicleRemove;

    private final VehicleSynchronize vehicleSynchronize;
    public ClojureSimulationFacade (final ClojureCarlaConnection carlaConnection) {
        this.carlaConnection = carlaConnection;
        this.simulateStep = carlaConnection.commandRegister.getOrCreate(SimulationSimulateStep.class);
        this.vehicleGet = carlaConnection.commandRegister.getOrCreate(VehicleGet.class);
        this.vehicleAdd = carlaConnection.commandRegister.getOrCreate(VehicleAdd.class);
        this.vehicleRemove = carlaConnection.commandRegister.getOrCreate(VehicleRemove.class);
        this.vehicleSynchronize = carlaConnection.commandRegister.getOrCreate(VehicleSynchronize.class);
    }

    public ClojureSimulationStepResult simulateUntil(long time, CoSimulationStep simulationStep) {
        simulateStep.execute(carlaConnection);

        Map<String, VehicleData> sumoVehicleDataMap = simulationStep.getVehicleUpdates().stream()
                .collect(Collectors.toMap(UnitData::getName, o -> o, (o1, o2) -> o1));

        /*
        CARLA -> SUMO
         */
        List<Long> spawnedActorIds = (List<Long>) java_api.copyToJVM(carlaConnection.spawnedCarlaActors);
        List<Long> destroyedActorIds = (List<Long>) java_api.copyToJVM(carlaConnection.destroyedActors);
        spawnedActorIds = new ArrayList<>(spawnedActorIds);
        spawnedActorIds.removeAll(carlaConnection.sumo2carlaIds.values());
        // spawn SUMO vehicle actor
        List<CarlaVehicleRegistration> adds = new ArrayList<>();
        for (Long spawnedActorId : spawnedActorIds) {
            Object carlaActor = vehicleGet.execute(carlaConnection, spawnedActorId);
            adds.add(encapsulation(time, spawnedActorId, carlaActor));
        }

        // destroy SUMO vehicle actor\
        List<String> removes = new ArrayList<>();
        for (Long destroyedActorId : destroyedActorIds) {
            if (carlaConnection.carla2SumoIds.containsKey(destroyedActorId)) {
                String vehicleId = carlaConnection.carla2SumoIds.remove(destroyedActorId);
                removes.add(vehicleId);
            }
        }

        // update SUMO vehicle actor
        List<CarlaVehicleData> updates = new ArrayList<>();
        Set<Map.Entry<Long, String>> entries = carlaConnection.carla2SumoIds.entrySet();
        for (Map.Entry<Long, String> entry : entries) {
            String vehicleId = entry.getValue();
            Long actorId = entry.getKey();
            Object carlaActor = vehicleGet.execute(carlaConnection, actorId);

            CarlaVehicleData carlaVehicleData;
            if (sumoVehicleDataMap.containsKey(vehicleId)) {
                VehicleData sumoVehicleData = sumoVehicleDataMap.get(vehicleId);
                carlaVehicleData = encapsulation(time, carlaActor, vehicleId, sumoVehicleData.getVehicleSignals().getCode());
            } else {
                carlaVehicleData = encapsulation(time, carlaActor, vehicleId, 0);
            }

            updates.add(carlaVehicleData);
        }

        return new ClojureSimulationStepResult(adds, removes, updates);
    }

    public void synchronizeVehicle(VehicleUpdates vehicleUpdates) {
        for (VehicleData vehicleData : vehicleUpdates.getAdded()) {
            if (!carlaConnection.carla2SumoIds.containsValue(vehicleData.getName())) {
                vehicleAdd.execute(carlaConnection, vehicleData);
                vehicleSynchronize.execute(carlaConnection, vehicleData);
            }
        }

        for (String removedName : vehicleUpdates.getRemovedNames()) {
            if (carlaConnection.sumo2carlaIds.containsKey(removedName)) {
                vehicleRemove.execute(carlaConnection, removedName);
            }
        }

        for (VehicleData vehicleData : vehicleUpdates.getUpdated()) {
            if (carlaConnection.sumo2carlaIds.containsKey(vehicleData.getName())) {
                vehicleSynchronize.execute(carlaConnection, vehicleData);
            }
        }
    }


    /*
    encapsulation interaction/data
     */
    private CarlaVehicleRegistration encapsulation(long time, Long carlaActorId, Object carlaActor) {
        try (AutoCloseable locker = java_api.GILLocker()) {
            String typeId = (String) java_api.copyToJVM(java_api.getAttr(carlaActor, "type_id"));
            Object attrs = java_api.getAttr(carlaActor, "attributes");
            String numberOfWheels = (String) java_api.copyToJVM(java_api.getItem(attrs, "number_of_wheels"));

            Map global = java_api.runStringAsFile(
                    "\n" +
                            "def get_vehicle_class(type_id):\n" +
                            "   if type_id in _VTYPES:\n" +
                            "       if 'vClass' in _VTYPES[type_id]:\n" +
                            "           return _VTYPES[type_id]['vClass']\n" +
                            "   return ''\n" +
                            "\n" +
                            "def get_shape_class(type_id):\n" +
                            "   if type_id in _VTYPES:\n" +
                            "       if 'guiShape' in _VTYPES[type_id]:\n" +
                            "           return _VTYPES[type_id]['guiShape']\n" +
                            "   return ''\n"
            );
            Object getVehicleClass = global.get("get_vehicle_class");
            Object getShapeClass = global.get("get_shape_class");
            String vehicleClass = (String) java_api.call(getVehicleClass, typeId);
            String shapeClass = (String) java_api.call(getShapeClass, typeId);

            String color = "";
            if (java_api.hasItem(carlaActor, "color")) {
                color = (String) java_api.copyToJVM(java_api.getItem(attrs, "color"));
            }

            Object extent = java_api.getAttr(java_api.getAttr(carlaActor, "bounding_box"), "extent");
            double length = (double) java_api.copyToJVM(java_api.getAttr(extent, "x"));
            double width = (double) java_api.copyToJVM(java_api.getAttr(extent, "y"));
            double height = (double) java_api.copyToJVM(java_api.getAttr(extent, "z"));

            String vehicleId = NameGenerator.getCarlaVehicleName();
            carlaConnection.carla2SumoIds.put(carlaActorId, vehicleId);

            return new CarlaVehicleRegistration(time,
                    vehicleId,
                    typeId,
                    color,
                    "carla_route",
                    Integer.parseInt(numberOfWheels),
                    2.0 * length,
                    2.0 * width,
                    2.0 * height,
                    vehicleClass,
                    shapeClass);
        } catch (Exception e) {
            throw  new RuntimeException(e);
        }
    }

    private CarlaVehicleData encapsulation(long time, Object carlaActor, String vehicleId, int currentSignals) {
        try (AutoCloseable locker = java_api.GILLocker()) {
            Integer signals = null;

            Map global = java_api.runStringAsFile(
                    "\n" +
                            "import carla\n" +
                            "import math\n" +
                            "class SumoVehSignal(object):\n" +
                            "    BLINKER_RIGHT = 1 << 0\n" +
                            "    BLINKER_LEFT = 1 << 1\n" +
                            "    BLINKER_EMERGENCY = 1 << 2\n" +
                            "    BRAKELIGHT = 1 << 3\n" +
                            "    FRONTLIGHT = 1 << 4\n" +
                            "    FOGLIGHT = 1 << 5\n" +
                            "    HIGHBEAM = 1 << 6\n" +
                            "    BACKDRIVE = 1 << 7\n" +
                            "    WIPER = 1 << 8\n" +
                            "    DOOR_OPEN_LEFT = 1 << 9\n" +
                            "    DOOR_OPEN_RIGHT = 1 << 10\n" +
                            "    EMERGENCY_BLUE = 1 << 11\n" +
                            "    EMERGENCY_RED = 1 << 12\n" +
                            "    EMERGENCY_YELLOW = 1 << 13 \n" +
                            "\n" +
                            "def get_sumo_lights_state(current_sumo_lights, carla_lights):\n" +
                            "        \"\"\"\n" +
                            "        Returns sumo signals based on carla vehicle light state.\n" +
                            "        \"\"\"\n" +
                            "        current_lights = current_sumo_lights\n" +
                            "\n" +
                            "        # Blinker right.\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.RightBlinker) !=\n" +
                            "                bool(current_lights & SumoVehSignal.BLINKER_RIGHT)):\n" +
                            "            current_lights ^= SumoVehSignal.BLINKER_RIGHT\n" +
                            "\n" +
                            "        # Blinker left.\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.LeftBlinker) !=\n" +
                            "                bool(current_lights & SumoVehSignal.BLINKER_LEFT)):\n" +
                            "            current_lights ^= SumoVehSignal.BLINKER_LEFT\n" +
                            "\n" +
                            "        # Emergency.\n" +
                            "        if (all([\n" +
                            "                bool(carla_lights & carla.VehicleLightState.RightBlinker),\n" +
                            "                bool(carla_lights & carla.VehicleLightState.LeftBlinker)\n" +
                            "        ]) != (current_lights & SumoVehSignal.BLINKER_EMERGENCY)):\n" +
                            "            current_lights ^= SumoVehSignal.BLINKER_EMERGENCY\n" +
                            "\n" +
                            "        # Break.\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.Brake) !=\n" +
                            "                bool(current_lights & SumoVehSignal.BRAKELIGHT)):\n" +
                            "            current_lights ^= SumoVehSignal.BRAKELIGHT\n" +
                            "\n" +
                            "        # Front (low beam)\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.LowBeam) !=\n" +
                            "                bool(current_lights & SumoVehSignal.FRONTLIGHT)):\n" +
                            "            current_lights ^= SumoVehSignal.FRONTLIGHT\n" +
                            "\n" +
                            "        # Fog light.\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.Fog) !=\n" +
                            "                bool(current_lights & SumoVehSignal.FOGLIGHT)):\n" +
                            "            current_lights ^= SumoVehSignal.FOGLIGHT\n" +
                            "\n" +
                            "        # High beam ligth.\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.HighBeam) !=\n" +
                            "                bool(current_lights & SumoVehSignal.HIGHBEAM)):\n" +
                            "            current_lights ^= SumoVehSignal.HIGHBEAM\n" +
                            "\n" +
                            "        # Backdrive (reverse)\n" +
                            "        if (bool(carla_lights & carla.VehicleLightState.Reverse) !=\n" +
                            "                bool(current_lights & SumoVehSignal.BACKDRIVE)):\n" +
                            "            current_lights ^= SumoVehSignal.BACKDRIVE\n" +
                            "\n" +
                            "        return current_lights" +
                            "\n" +
                            "def get_sumo_transform(carla_actor):\n" +
                            "        in_carla_transform = carla_actor.get_transform()\n" +
                            "        extent = carla_actor.bounding_box.extent\n" +
                            "        in_location = in_carla_transform.location\n" +
                            "        in_rotation = in_carla_transform.rotation\n" +
                            "\n" +
                            "        # From center to front-center-bumper (carla reference system).\n" +
                            "        yaw = -1 * in_rotation.yaw\n" +
                            "        pitch = in_rotation.pitch\n" +
                            "        out_location = (in_location.x + math.cos(math.radians(yaw)) * extent.x,\n" +
                            "                        in_location.y - math.sin(math.radians(yaw)) * extent.x,\n" +
                            "                        in_location.z - math.sin(math.radians(pitch)) * extent.x)\n" +
                            "        out_rotation = (in_rotation.pitch, in_rotation.yaw, in_rotation.roll)\n" +
                            "\n" +
                            "        # Applying offset carla-sumo net\n" +
                            "        out_location = (out_location[0] + offset[0], out_location[1] - offset[1], out_location[2])\n" +
                            "\n" +
                            "        # Transform to sumo reference system.\n" +
                            "        out_transform = carla.Transform(\n" +
                            "            carla.Location(out_location[0], -out_location[1], out_location[2]),\n" +
                            "            carla.Rotation(out_rotation[0], out_rotation[1] + 90, out_rotation[2]))\n" +
                            "\n" +
                            "        return out_transform"
            );
            if (carlaConnection.syncVehicleLights) {
                Object getCarlaLightsState = java_api.getAttr(carlaActor, "get_actor_light_state");
                Object carlaLight = java_api.call(getCarlaLightsState);

                Object getSumoLightsState = global.get("get_sumo_lights_state");

                Object signalsPy = java_api.call(getSumoLightsState, currentSignals, carlaLight);
                signals = (int) java_api.copyToJVM(signalsPy);
            }

            Object getSumoTransform = global.get("get_sumo_transform");
            Object outTransform = java_api.call(getSumoTransform, carlaActor);

            Object locationAttr = java_api.getAttr(outTransform, "location");
            Object rotationAttr = java_api.getAttr(outTransform, "rotation");

            double x = (double) java_api.copyToJVM(java_api.getAttr(locationAttr, "x"));
            double y = (double) java_api.copyToJVM(java_api.getAttr(locationAttr, "y"));
            double heading = (double) java_api.copyToJVM(java_api.getAttr(rotationAttr, "yaw"));

            MutableCartesianPoint point = new MutableCartesianPoint(x, y, 0);

            return new CarlaVehicleData(time, vehicleId, point.toGeo(), signals, heading);

        } catch (Exception e) {
            throw new RuntimeException(e);
        }
    }
}
